/*
 *  DLQRSolver.cpp
 *  FaceTracker
 *
 *  Created by Kenn Sebesta on 2007-11-22.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

// Discrete LQR solver. This returns the matrix K_dlqr, which is the optimal 
// control for the given matrices A, B, Q, and R. 
// ATTENTION: A and B are in DISCRETE TIME.

#include "dLQRSolver.h"
#include "displayOpenCV.h"

/*
 The algorithm for solving the riccati eqn. comes from "NUMERICAL SOLUTION OF DISCRETE-TIME ALGEBRAIC RICCATI EQUATION" by Tan K. Nguyen
 
 X=Q+A_t*(B*R^-1*B_t)^-1*A 
 for i=1:10000
 X=Q+A_t*(X^-1+B*R^-1*B_t)^-1*A
 
 */

CvMat* dLQRSolver( CvMat *A, CvMat *B, CvMat *Q, CvMat *R )
{
	CvMat *R_inv, *A_trnsp, *B_trnsp, *X, *X_inv, *BR, *BRB, *tmp_inv, *K_dlqr;

	// Initialize matrices
	A_trnsp=cvCreateMat(4,4,CV_32F);	
	B_trnsp=cvCreateMat(2,4,CV_32F);
	BR=cvCreateMat(4,2,CV_32F);
	R_inv=cvCreateMat(2,2,CV_32F);
	X=cvCreateMat(4,4,CV_32F);
	X_inv=cvCreateMat(4,4,CV_32F);
	BRB=cvCreateMat(4,4,CV_32F);
	tmp_inv=cvCreateMat(4,4,CV_32F);
	K_dlqr=cvCreateMat(2,4,CV_32F);
	
	// Precalculate Matrices
	cvTranspose( A, A_trnsp );
	cvTranspose( B, B_trnsp );
	cvInvert( R, R_inv, CV_SVD_SYM );
	cvMatMul( B, R_inv, BR );
	cvMatMul( BR, B_trnsp, BRB );
	printCvArr(BRB);
	// Calculate X_1, the seeding value for the riccati eqn. solution.
	cvInvert( BRB, tmp_inv, CV_SVD );
	printCvArr(tmp_inv);
	cvMatMul( BRB, tmp_inv, BRB );        
	printCvArr(BRB);
        cvWaitKey(0);
	cvMatMul( A_trnsp, tmp_inv, tmp_inv );
	cvMatMulAdd( tmp_inv, A, Q, tmp_inv );
	
	// Calculate X_n, the convergent value for the riccati eqn. solution.
	for (int i=1; i<10001; i++)
	{
		cvInvert(X, X_inv, CV_SVD);
		cvAdd(X_inv,BRB,tmp_inv);
		cvInvert(tmp_inv, tmp_inv, CV_SVD);
		cvMatMul( A_trnsp, tmp_inv, tmp_inv );
		cvMatMulAdd( tmp_inv, A, Q, X );
	}
	
	// Calculate K_dlqr
	cvMatMul(X,A,tmp_inv);
	cvMatMul(B_trnsp,tmp_inv,K_dlqr);
	cvMatMul(X,B,BR);
	cvMatMulAdd(B_trnsp,BR,R,R_inv);
	cvInvert(R_inv,R_inv, CV_SVD);
	cvMatMul(R_inv,K_dlqr,K_dlqr);
	
	// Garbage collection
	cvReleaseMat(&R_inv);
	cvReleaseMat(&A_trnsp);
	cvReleaseMat(&B_trnsp);
	cvReleaseMat(&BR);
	cvReleaseMat(&X);
	cvReleaseMat(&X_inv);
	cvReleaseMat(&BRB);
	cvReleaseMat(&tmp_inv);
	
	return K_dlqr;
}
